#include "CSerialPort.h"
#include "cylinder.h"
#include "getAmmo.h"
#include <netinet/in.h>
#include<sys/ioctl.h>
#include <thread>
#include <termios.h>
 
using namespace std;
typedef pcl::PointXYZ PointT;

//-------------------CSerialPort & thread---------------------------
static unsigned char re_msg =  0x02;
char mode = 0x02;
void ReadSerial();

int fd;
fd_set fs_read;
struct termios option;
CSerialPort myserialport(fd, option, fs_read);
char send_data[8];

//-------------------function---------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr points_to_pcl(const rs2::points& points,float y_min,float y_max);
void keyinput();
void enable_raw_mode();
void disable_raw_mode();

//-------------------realsense & global variable---------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_fil(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<PointT>::Ptr cloud_viewer(new pcl::PointCloud<PointT>);
short flag=0;

pcl::visualization::CloudViewer viewer("viewer");

float y_min_cylinder,y_max_cylinder;
float y_min_getammo,y_max_getammo;
char c='1';
 pcl::console::TicToc tt;	

//-------------------cylinder---------------------------
float y_min;
typedef pcl::PointXYZ PointT;
pcl::PassThrough<PointT> pass_cylinder;                         
pcl::VoxelGrid<PointT> sor_cylinder;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg_cylinder; 
pcl::EuclideanClusterExtraction<PointT> ec;
pcl::PointCloud<PointT>::Ptr cloud_filtered_cylinder(new pcl::PointCloud<PointT>);
pcl::search::KdTree<PointT>::Ptr tree_cylinder(new pcl::search::KdTree<PointT>());
pcl::search::KdTree<PointT>::Ptr tmp_tree(new pcl::search::KdTree<PointT>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<PointT>::Ptr cloud_filtered_plane(new pcl::PointCloud<PointT>);
pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);
pcl::PointCloud<pcl::PointXYZ>::Ptr all_cloud_cluster_cylinder(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());
                     
pcl::PassThrough<PointT> pass_plane;                         
pcl::VoxelGrid<PointT> sor_plane;    
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::ExtractIndices<PointT> extract;

//-------------------getAmmo---------------------------
pcl::PassThrough<PointT> pass_getAmmo;                         //直通滤波对象
pcl::VoxelGrid<PointT> sor_getAmmo;
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
std::vector<pcl::PointIndices> cluster_indices;
pcl::MomentOfInertiaEstimation <PointT> feature_extractor;
pcl::search::Search<pcl::PointXYZ>::Ptr tree_getAmmo = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;

pcl::PointCloud<PointT>::Ptr cloud_filtered_getAmmo(new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr all_cloud_cluster(new pcl::PointCloud<PointT>);

short x,angle, z;
//----------------------------------------------------

int main()
{
	std::thread *read_thresd = new std::thread(ReadSerial);
    thread task1(keyinput);
	y_min_getammo=-0.0295;y_max_getammo=0.033;
	y_min_cylinder=-0.08;y_max_cylinder=0.15;
	while (true)
	{
		rs2::context devices_list;
		//cout<<"0 ";
		flag=(devices_list.query_all_sensors()).size();
		//cout<<"1 ";
		while(flag==0)
			{
		//cout<<"2 ";
				rs2::context devices_list;
				flag=(devices_list.query_all_sensors()).size();
				usleep(1000);
		//cout<<"3 ";
			}		
		rs2::pointcloud pc;
		rs2::points points;
		rs2::pipeline pipe;
		pipe.start();

		//cout<<"4 ";
		std::string title;
		int n=0;
		
		while(true)
		{
			tt.tic();
			rs2::context devices_list;
			if((devices_list.query_all_sensors()).size()==0) break;

			auto frames = pipe.wait_for_frames();
			auto depth = frames.get_depth_frame();
		    points = pc.calculate(depth);
			if (mode == 0x02)//getAmmo
			{
				cloud_filtered = points_to_pcl(points,y_min_getammo,y_max_getammo);                                                                                                         
				getammo_run();
				
			}
			if(mode ==0x03)//cylinder
			{
				cloud_filtered = points_to_pcl(points,y_min_cylinder,y_max_cylinder);
				findcylinder_run();                                                                                               
			}
			if(mode ==0x04)//cylinder
			{
				cloud_filtered = points_to_pcl(points,-2,2);
				pcl::PointCloud<pcl::PointXYZ> cloud1;
				cloud1.operator+=(*cloud_filtered);

				time_t rawtime;
				struct tm * ptm;
				time ( &rawtime );
				ptm = gmtime ( &rawtime );

				string title=to_string(ptm->tm_yday)+to_string(ptm->tm_hour)+to_string(ptm->tm_min)+to_string(ptm->tm_sec);
				title = "/home/ubuntu/桌面/point-eng/"+ title + ".pcd";
				
				if(cloud1.size()>0)pcl::io::savePCDFileASCII (title, cloud1); //将点云保存到PCD文件中
				viewer.showCloud(cloud_filtered);
				usleep(100000);	
			}
			myserialport.writeToCom_char((char*)&send_data, sizeof(send_data));
			cout<<"senddata:"<<int(send_data[0])<<","<<int(send_data[1])<<","<<int(send_data[2])<<","<<int(send_data[3])<<","<<int(send_data[4])<<","<<int(send_data[5])<<","<<int(send_data[6])<<","<<int(send_data[7])<<endl;
			viewer.showCloud(cloud_viewer);
		}
	}
}

void ReadSerial()
{
	while (1)
	{
		int n = myserialport.readFromCom_uchar(&re_msg,1);
		if (re_msg == 0x01)
		{
			mode = 0x01;
		}
		else if (re_msg == 0x02)
		{
			mode = 0x02;
		}
		else if(re_msg == 0x03)
		{
			mode = 0x03;
		}
		else if(re_msg == 0x04)
		{
			mode = 0x04;
		}
		usleep(1000);
	}
}

pcl::PointCloud<pcl::PointXYZ>::Ptr points_to_pcl(const rs2::points& points,float y_min,float y_max)
{
    auto sp = points.get_profile().as<rs2::video_stream_profile>();
    cloud->width = sp.width();
    cloud->height = sp.height();
    cloud->is_dense = true;
    cloud->points.resize(points.size());
	cloud_fil->points.resize(0);
   
    auto ptr = points.get_vertices();
	
    for (auto& p : cloud->points)
    {
		
		p.x = ptr->x;
        p.y = ptr->y;
        p.z = ptr->z;
		
       
		if(ptr->y>y_min&&ptr->y<y_max)
		{
		cloud_fil->points.push_back(p);
		}
		 ptr++;
    }
	
    return cloud_fil;
}

void enable_raw_mode()
{
    termios term;
    tcgetattr(0, &term);
    term.c_lflag &= ~(ICANON | ECHO); // Disable echo as well
    tcsetattr(0, TCSANOW, &term);
}

void disable_raw_mode()
{
    termios term;
    tcgetattr(0, &term);
    term.c_lflag |= ICANON | ECHO;
    tcsetattr(0, TCSANOW, &term);
}

void keyinput()
{
   enable_raw_mode();
    while(1)
    {
		
        usleep(10);
		c=getchar();
		if(c=='1'||c=='2'||c=='3'||c=='4')
		{	
			if(c=='2')
			{
				mode=re_msg=0x02;
			}
			else if(c=='3')
			{
				mode=re_msg=0x03;
			}
			else if(c=='4')
			{
				mode=re_msg=0x04;
			}
			else if(c=='1')
			{
				mode=re_msg=0x01;
			}
		}
		else if(c=='z') 
		{
			cout<<"\ny_min_getammo,y_max_getammo:"<<y_min_getammo<<","<<y_max_getammo<<endl;
			cout<<"y_min_cylinder,y_max_cylinder:"<<y_min_cylinder<<","<<y_max_cylinder<<endl;
			struct termios te;
			int ch;
			tcgetattr( STDIN_FILENO,&te);
			te.c_lflag &=~( ICANON|ECHO);
			tcsetattr(STDIN_FILENO,TCSANOW,&te);
			tcflush(STDIN_FILENO,TCIFLUSH);
			fgetc(stdin) ; 
			te.c_lflag |=( ICANON|ECHO);
			tcsetattr(STDIN_FILENO,TCSANOW,&te);
		}
		else if(c=='q') y_min_getammo+=0.005;
		else if(c=='a') y_min_getammo-=0.005;
		else if(c=='w') y_max_getammo+=0.005;
		else if(c=='s') y_max_getammo-=0.005;
		else if(c=='o') y_min_cylinder+=0.005;
		else if(c=='k') y_min_cylinder-=0.005;
		else if(c=='p') y_max_cylinder+=0.005;
		else if(c=='l') y_max_cylinder-=0.005;
		
    }
	disable_raw_mode();
    tcflush(0, TCIFLUSH); // Clear stdin to prevent characters appearing on prompt
}
